#include "em80_driver.h"

#include <cassert>
#include <cmath>
#include <sstream>

#include <boost/bind.hpp>


namespace em80_driver
{

EM80Driver::EM80Driver(ros::NodeHandle nh, ros::NodeHandle private_nh) :
    em80_(std::string(),
          boost::bind(&EM80Driver::em80PayloadCallback, this, _1),
          boost::bind(&EM80Driver::em80ErrorCallback, this, _1))
{
    //get em80 serial port address
    std::string port;
    if(!private_nh.getParam("port", port))
    {
        ROS_FATAL("EM80 communication port parameter required.");
        ros::shutdown();
        return;
    }

    // attempt to connect to the serial port
    try
    {
        em80_.connect(port);
    }
    catch (SerialException e)
    {
        ROS_FATAL("Failed to connect to the EM80, %s.", e.what());
        ros::shutdown();
        return;
    }


    // create em80 publishers
    //odom publish
    odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 5);

    //imu publish
    imu_pub = nh.advertise<sensor_msgs::Imu>("sensors/imu", 20);

    //ultrasonic publish
    ultrasonic_pub_1 = nh.advertise<sensor_msgs::Range>("sensors/ultrasonic_1",20);
    ultrasonic_pub_2 = nh.advertise<sensor_msgs::Range>("sensors/ultrasonic_2",20);
    ultrasonic_pub_3 = nh.advertise<sensor_msgs::Range>("sensors/ultrasonic_3",20);
    ultrasonic_pub_4 = nh.advertise<sensor_msgs::Range>("sensors/ultrasonic_4",20);
    ultrasonic_pub_5 = nh.advertise<sensor_msgs::Range>("sensors/ultrasonic_5",20);
    ultrasonic_pub_6 = nh.advertise<sensor_msgs::Range>("sensors/ultrasonic_6",20);
    ultrasonic_pub_7 = nh.advertise<sensor_msgs::Range>("sensors/ultrasonic_7",20);
    ultrasonic_pub_8 = nh.advertise<sensor_msgs::Range>("sensors/ultrasonic_8",20);

    
    
    // core_sensors_pub = nh.advertise<em80_msgs::CoreSensors>("sensors/core", 0);
    docking_pub = nh.advertise<em80_msgs::Docking>("sensors/docking", 0);
    battery_pub = nh.advertise<em80_msgs::Battery>("sensors/battery", 0);
    gp_input_pub = nh.advertise<em80_msgs::GPInput>("sensors/gp_input", 0);
    adc_pub = nh.advertise<em80_msgs::ADC>("sensors/adc", 0);
    firmware_pub = nh.advertise<em80_msgs::Firmware>("firmware_info", 0);
    status_pub = nh.advertise<em80_msgs::Status>("status", 10);

    // subscribe to the commands
    velocity_sub = nh.subscribe("cmd_vel", 20, &EM80Driver::velocityCallback, this);
    
    
    docking_config_sub = nh.subscribe("commands/docking_config", 10, &EM80Driver::dockingConfigCallback, this);
    led_config_sub = nh.subscribe("commands/led_config", 10, &EM80Driver::ledConfigCallback, this);
    gpio_config_sub = nh.subscribe("commands/gpio_config", 10, &EM80Driver::gpioConfigCallback, this);
    adc_config_sub = nh.subscribe("commands/adc_config", 10, &EM80Driver::adcConfigCallback, this);
    dac_config_sub = nh.subscribe("commands/dac_config", 10, &EM80Driver::dacConfigCallback, this);
    power_source_config_sub = nh.subscribe("commands/power_source_config_sub", 10, &EM80Driver::powerSourceConfigCallback, this);
    joystick_mode_sub = nh.subscribe("commands/joystick_mode", 10, &EM80Driver::joystickModeCallback, this);
    firmware_enquiry_sub = nh.subscribe("commands/firmware_enquriy", 10, &EM80Driver::firmwareEnquiryCallback, this);
    docking_enquiry_sub = nh.subscribe("commands/docking_enquiry", 10, &EM80Driver::dockingEnquiryCallback, this);
    status_enquiry_sub = nh.subscribe("commands/status_enquriy", 10, &EM80Driver::statusEnquiryCallback, this);

    // create a 50Hz timer, used for check the polling EM80 telemetry

}

void EM80Driver::em80PayloadCallback(const boost::shared_ptr<EM80PayloadBase const> &payload)
{
    //TODO: TOO UGLY!!!! using an injection method
    if(payload->payloadName() == "ADC")
    {
        em80_msgs::ADC::Ptr adc_msg(new em80_msgs::ADC);
        boost::shared_ptr<ADC const> adc = boost::dynamic_pointer_cast<ADC const>(payload);

        adc_msg->value = adc->data.value;


        adc_pub.publish(adc_msg);
    }
    else if(payload->payloadName() == "CoreSensors")
    {
        em80_msgs::CoreSensors::Ptr coreSensors_msg(new em80_msgs::CoreSensors);
        boost::shared_ptr<CoreSensors const> coreSensors = boost::dynamic_pointer_cast<CoreSensors const>(payload);
       
        /***********************************Odom************************************/ 

        static tf::TransformBroadcaster odom_broadcaster;//定义tf对象
    	geometry_msgs::TransformStamped odom_trans;//创建一个tf发布需要使用的TransformStamped类型消息
        nav_msgs::Odometry odom;//定义里程计对象
        
    	//定义covariance矩阵，作用为解决position和速度的不同测量的不确定性
        float covariance[36] = {var_len,   0,    0,     0,     0,     0,  // covariance on gps_x
                            0,  var_len, 0,     0,     0,     0,  // covariance on gps_y
                            0,  0,    99999, 0,     0,     0,  // covariance on gps_z
                            0,  0,    0,     99999, 0,     0,  // large covariance on rot x
                            0,  0,    0,     0,     99999, 0,  // large covariance on rot y
                            0,  0,    0,     0,     0,     var_angle};  // large covariance on rot z
        //载入covariance矩阵
        for(int i = 0; i < 36; i++)
        {
            odom.pose.covariance[i] = covariance[i];
            odom.twist.covariance[i] = covariance[i];
        }

        position_x_temp = coreSensors->data.odometry_x;
        position_y_temp = coreSensors->data.odometry_y;
        oriention_temp = coreSensors->data.odometry_th;
        vel_linear_temp = coreSensors->data.linear_vel;
        vel_angular_temp = coreSensors->data.imu_alpha;
        
        position_x = static_cast<float>(position_x_temp);
        position_y = static_cast<float>(position_y_temp);
        oriention =  static_cast<float>(oriention_temp) / 100;
        vel_linear = static_cast<float>(vel_linear_temp);
        vel_angular = static_cast<float>(vel_angular_temp);
        
        //将X，Y坐标，线速度缩小1000倍
        position_x /= 1000; // m
        position_y /= 1000; // m
	    vel_linear /= 1000; // m/s 

        //printf("linear velocity: %f \n", vel_linear);
        /*
        if(oriention > 180 && oriention < 360)
        {
            oriention = (360.0f - oriention);
        }
        else
        {
            oriention = -oriention;
        }
        */
        
	    static tf::TransformBroadcaster br;
        tf::Quaternion q;
        tf::Transform transform;
        transform.setOrigin( tf::Vector3(position_x, position_y, 0.0) );
        q.setRPY(0, 0,oriention/180.0f*pi);
        transform.setRotation(q);
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_footprint"));//使用rf2o里程计，需要屏蔽odom和tf
        
       
        //载入里程计时间戳
        odom.header.stamp = ros::Time::now();
        //里程计的父子坐标系
        odom.header.frame_id = "/odom";
        odom.child_frame_id = "/base_footprint";
        //里程计位置数据：x,y,z,方向
        odom.pose.pose.position.x = position_x;
        odom.pose.pose.position.y = position_y;
        odom.pose.pose.position.z = 0.0;

        geometry_msgs::Quaternion odom_quat; //四元数变量
        //里程计的偏航角需要转换成四元数来发布
	    odom_quat = tf::createQuaternionMsgFromYaw(oriention/180.0f*pi);//将偏航角转换成四元数
        odom.pose.pose.orientation = odom_quat;
        
        //载入线速度和角速度
        odom.twist.twist.linear.x = vel_linear * cos(oriention/180.0f*pi);
        odom.twist.twist.linear.y = vel_linear * sin(oriention/180.0f*pi);
        odom.twist.twist.angular.z = vel_angular / 180.0f* pi;
        //发布里程计
        odom_pub.publish(odom);

        /**************************************IMU**************************************/

        l_acc_x_temp = coreSensors -> data.acc_x;
        l_acc_y_temp = coreSensors -> data.acc_y;
        l_acc_z_temp = coreSensors -> data.acc_z;
        l_acc_x = static_cast<float>(l_acc_x_temp); 
        l_acc_y = static_cast<float>(l_acc_y_temp); 
        l_acc_z = static_cast<float>(l_acc_z_temp); 
        l_acc_x /= 1000.0f; // unit: g
        l_acc_y /= 1000.0f; // unit: g
        l_acc_z /= 1000.0f; // unit: g


        imu_data.header.stamp = ros::Time::now();
        imu_data.header.frame_id = "base_link";
        imu_data.orientation = odom_quat;
        
        imu_data.angular_velocity.x = 0.0f;
        imu_data.angular_velocity.y = 0.0f;
        imu_data.angular_velocity.z = vel_angular / 180.0f * pi;

        imu_data.linear_acceleration.x = l_acc_x;
        imu_data.linear_acceleration.y = l_acc_y;
        imu_data.linear_acceleration.z = l_acc_z;
        
        imu_pub.publish(imu_data);





        /**************************************Ultrasonic**************************************/

        sonar0_tmp = coreSensors -> data.ultrasonic_1;
        sonar1_tmp = coreSensors -> data.ultrasonic_2;
        sonar2_tmp = coreSensors -> data.ultrasonic_3;
        sonar3_tmp = coreSensors -> data.ultrasonic_4;
        sonar4_tmp = coreSensors -> data.ultrasonic_5;
        sonar5_tmp = coreSensors -> data.ultrasonic_6;
        sonar6_tmp = coreSensors -> data.ultrasonic_7;
        sonar7_tmp = coreSensors -> data.ultrasonic_8;
	

        //sonic test
	/*
	printf("******************************************************\n");
	printf("sonar1: %d \n", sonar0_tmp);
	printf("sonar2: %d \n", sonar1_tmp);
	printf("sonar3: %d \n", sonar2_tmp);
	printf("sonar4: %d \n", sonar3_tmp);
	printf("sonar5: %d \n", sonar4_tmp);
	printf("sonar6: %d \n", sonar5_tmp);
	printf("sonar7: %d \n", sonar6_tmp);
	printf("sonar8: %d \n", sonar7_tmp);
	printf("******************************************************\n");
	*/

        //  datatype convert
		sonar0_range = (static_cast<float>(sonar0_tmp))/1000;
		sonar1_range = (static_cast<float>(sonar1_tmp))/1000;
		sonar2_range = (static_cast<float>(sonar2_tmp))/1000;
		sonar3_range = (static_cast<float>(sonar3_tmp))/1000;
		sonar4_range = (static_cast<float>(sonar4_tmp))/1000;
		sonar5_range = (static_cast<float>(sonar5_tmp))/1000;
		sonar6_range = (static_cast<float>(sonar6_tmp))/1000;
		sonar7_range = (static_cast<float>(sonar7_tmp))/1000;

        //sonar0
		sonar0.header.stamp = ros::Time::now();
        sonar0.header.frame_id = "Ultrasonic_1";
		sonar0.radiation_type = SONAR_SENSOR;
		sonar0.field_of_view = SONAR_VIEW;
		sonar0.min_range = SONAR_RANGE_MIN;
		sonar0.max_range = SONAR_RANGE_MAX;
		sonar0.range = sonar0_range;
        ultrasonic_pub_1.publish(sonar0);

		//sonar1
		sonar1.header.stamp = ros::Time::now();
        sonar1.header.frame_id = "Ultrasonic_2";
		sonar1.radiation_type = SONAR_SENSOR;
		sonar1.field_of_view = SONAR_VIEW;
		sonar1.min_range = SONAR_RANGE_MIN;
		sonar1.max_range = SONAR_RANGE_MAX;
		sonar1.range = sonar1_range;
        ultrasonic_pub_2.publish(sonar1);

		//sonar2
		sonar2.header.stamp = ros::Time::now();
        sonar2.header.frame_id = "Ultrasonic_3";
		sonar2.radiation_type = SONAR_SENSOR;
		sonar2.field_of_view = SONAR_VIEW;
		sonar2.min_range = SONAR_RANGE_MIN;
		sonar2.max_range = SONAR_RANGE_MAX;
		sonar2.range = sonar2_range;
        ultrasonic_pub_3.publish(sonar2);

		//sonar3
		sonar3.header.stamp = ros::Time::now();
        sonar3.header.frame_id = "Ultrasonic_4";
		sonar3.radiation_type = SONAR_SENSOR;
		sonar3.field_of_view = SONAR_VIEW;
		sonar3.min_range = SONAR_RANGE_MIN;
		sonar3.max_range = SONAR_RANGE_MAX;
		sonar3.range = sonar3_range;
        ultrasonic_pub_4.publish(sonar3);

		//sonar4
		sonar4.header.stamp = ros::Time::now();
        sonar4.header.frame_id = "Ultrasonic_5";
		sonar4.radiation_type = SONAR_SENSOR;
		sonar4.field_of_view = SONAR_VIEW;
		sonar4.min_range = SONAR_RANGE_MIN;
		sonar4.max_range = SONAR_RANGE_MAX;
		sonar4.range = sonar4_range;
        ultrasonic_pub_5.publish(sonar4);

		//sonar5
		sonar5.header.stamp = ros::Time::now();
        sonar5.header.frame_id = "Ultrasonic_6";
		sonar5.radiation_type = SONAR_SENSOR;
		sonar5.field_of_view = SONAR_VIEW;
		sonar5.min_range = SONAR_RANGE_MIN;
		sonar5.max_range = SONAR_RANGE_MAX;
		sonar5.range = sonar5_range;
        ultrasonic_pub_6.publish(sonar5);

		//sonar6
		sonar6.header.stamp = ros::Time::now();
        sonar6.header.frame_id = "Ultrasonic_7";
		sonar6.radiation_type = SONAR_SENSOR;
		sonar6.field_of_view = SONAR_VIEW;
		sonar6.min_range = SONAR_RANGE_MIN;
		sonar6.max_range = SONAR_RANGE_MAX;
		sonar6.range = sonar6_range;
        ultrasonic_pub_7.publish(sonar6);

		//sonar7
		sonar7.header.stamp = ros::Time::now();
        sonar7.header.frame_id = "Ultrasonic_8";
		sonar7.radiation_type = SONAR_SENSOR;
		sonar7.field_of_view = SONAR_VIEW;
		sonar7.min_range = SONAR_RANGE_MIN;
		sonar7.max_range = SONAR_RANGE_MAX;
		sonar7.range = sonar7_range;
        ultrasonic_pub_8.publish(sonar7);


        // coreSensors_msg->imu_th = coreSensors->data.imu_th;
        // coreSensors_msg->bumper = coreSensors->data.bumper;
        // coreSensors_msg->cliff = coreSensors->data.cliff;
        // coreSensors_msg->left_encoder = coreSensors->data.left_encoder;
        // coreSensors_msg->right_encoder = coreSensors->data.right_encoder;
        // core_sensors_pub.publish(coreSensors_msg);

    }
    else if(payload->payloadName() == "Battery")
    {
        em80_msgs::Battery::Ptr battery_msg(new em80_msgs::Battery);
        boost::shared_ptr<Battery const> battery = boost::dynamic_pointer_cast<Battery const>(payload);

        battery_msg->status = battery->data.status;
        battery_msg->capacity = battery->data.capacity;
        battery_msg->voltage = battery->data.voltage;

        battery_pub.publish(battery_msg);
    }
    else if(payload->payloadName() == "Docking")
    {
        em80_msgs::Docking::Ptr docking_msg(new em80_msgs::Docking);
        boost::shared_ptr<Docking const> docking = boost::dynamic_pointer_cast<Docking const>(payload);

        docking_msg->status = docking->data.status;

        docking_pub.publish(docking_msg);
    }
    else if(payload->payloadName() == "Firmware")
    {
        em80_msgs::Firmware::Ptr firmware_msg(new em80_msgs::Firmware);
        boost::shared_ptr<Firmware const> firmware = boost::dynamic_pointer_cast<Firmware const>(payload);

        firmware_msg->software_patch_version = firmware->data.software_patch_version;
        firmware_msg->software_minor_version = firmware->data.software_minor_version;
        firmware_msg->software_major_version = firmware->data.software_major_version;
        firmware_msg->hardware_patch_version = firmware->data.hardware_patch_version;
        firmware_msg->hardware_minor_version = firmware->data.hardware_minor_version;
        firmware_msg->hardware_major_version = firmware->data.hardware_major_version;
        firmware_msg->udid0 = firmware->data.udid0;
        firmware_msg->udid1 = firmware->data.udid1;
        firmware_msg->udid2 = firmware->data.udid2;

        firmware_pub.publish(firmware_msg);
    }
    else if(payload->payloadName() == "GPInput")
    {
        
        em80_msgs::GPInput::Ptr gp_input_msg(new em80_msgs::GPInput);
        boost::shared_ptr<GPInput const> gp_input = boost::dynamic_pointer_cast<GPInput const>(payload);
       
        gp_input_msg->button = gp_input->data.button;
        gp_input_msg->gp_input = gp_input->data.gp_input;

        gp_input_pub.publish(gp_input_msg);


    }
    else if(payload->payloadName() == "Status")
    {
        em80_msgs::Status::Ptr status_msg(new em80_msgs::Status);
        boost::shared_ptr<Status const> status = boost::dynamic_pointer_cast<Status const>(payload);

        status_msg->left_servo_error = status->data.left_servo_error;
        status_msg->right_servo_error = status->data.right_servo_error;
        status_msg->imu_error = status->data.imu_error;
        status_msg->battery_error = status->data.battery_error;
        status_msg->ultrasonic_warning = status->data.ultrasonic_warning;
        
        status_pub.publish(status_msg);
    }
}

void EM80Driver::em80ErrorCallback(const std::string &error)
{
    ROS_ERROR("%s", error.c_str());
}


void EM80Driver::velocityCallback(const geometry_msgs::Twist &velocity_config)
{
    float angular_z = 0, linear_x = 0;//暂存的线速度和角速度

	linear_x =  velocity_config.linear.x;//获取/cmd_vel的线速度.m/s
	angular_z = velocity_config.angular.z;//获取/cmd_vel的角速度,rad/s

	//D为两轮间距,单位为m
	int16_t l = static_cast<int16_t>((linear_x - 0.5f*angular_z*D)*ratio);//放大１０００倍，m/s-->mm/s
	int16_t r = static_cast<int16_t>((linear_x + 0.5f*angular_z*D)*ratio);//放大１０００倍，m/s-->mm/s


    em80_.SetVelocityControl(l, r);
    em80_.sendCommand();
}

void EM80Driver::dockingConfigCallback(const em80_msgs::DockingConfigConstPtr &docking_config)
{
    if(docking_config->mode)
    {
        em80_.SetDocking(DockingConfig::Mode::PROC);
        em80_.sendCommand();
    }
    else
    {
        em80_.SetDocking(DockingConfig::Mode::EXIT);
        em80_.sendCommand();
    }    
}

void EM80Driver::ledConfigCallback(const em80_msgs::LEDConfigConstPtr &led_config)
{
    em80_.SetLEDArray(static_cast<LEDConfig::Mode::State>(led_config->mode));
    em80_.sendCommand();
}

void EM80Driver::gpioConfigCallback(const em80_msgs::GPIOConfigConstPtr &gpio_config)
{
    em80_.SetGPIO(gpio_config->io_config, gpio_config->value);
    em80_.sendCommand();
}

void EM80Driver::adcConfigCallback(const em80_msgs::ADCConfigConstPtr &adc_config)
{
    em80_.SetADC(static_cast<ADCConfig::Control::State>(adc_config->adc));
    em80_.sendCommand();
}

void EM80Driver::dacConfigCallback(const em80_msgs::DACConfigConstPtr &dac_config)
{
    em80_.SetDAC(static_cast<DACConfig::Control::State>(dac_config->dac), dac_config->values);
    em80_.sendCommand();
}

void EM80Driver::powerSourceConfigCallback(const em80_msgs::PowerSourceConfigConstPtr &power_source_config)
{
    em80_.SetExternalPower(power_source_config->flags);
    em80_.sendCommand();
}

void EM80Driver::joystickModeCallback(const em80_msgs::JoystickModeConstPtr &joystick_mode)
{
    em80_.SetJoystick(static_cast<JoystickMode::Mode::State>(joystick_mode->mode));
    em80_.sendCommand();
}

void EM80Driver::firmwareEnquiryCallback(const em80_msgs::FWVersionEnquiryConstPtr &/*firmware_enquiry*/)
{
    em80_.GetExtended();
    em80_.sendCommand();
}

void EM80Driver::dockingEnquiryCallback(const em80_msgs::DockingEnquiryConstPtr &/*docking_enquiry*/)
{
    em80_.GetExtended();
    em80_.sendCommand();
}

void EM80Driver::statusEnquiryCallback(const em80_msgs::StatusEnquiryConstPtr &/*status_enquiry*/)
{
    em80_.GetExtended();
    em80_.sendCommand();
}

}
